#include "RollerSubsystem.h"
#include "EntechConstants.h"

RollerSubsystem::RollerSubsystem()
{
	m_armMotor = new Talon(c_rollerArmTalonChannel);
	
	// Switches shared between Prongs and Roller in order to act simultaneously
	// (Unless override switch is active)
	m_pickupOverride = new OperatorButton(c_pickupOverride);
	m_prongsUpButton = new OperatorButton(c_prongsUpButton);
	m_prongsDownButton = new OperatorButton(c_prongsDownButton);
	m_armBackSwitch = new OperatorButton(c_rollerArmBackSwitch);
	m_armDeployedSwitch = new OperatorButton(c_rollerArmDeployedSwitch);
	m_backLimitSwitch = new DigitalInput(c_rollerArmBackLimit);
	m_deployedLimitSwitch = new DigitalInput(c_rollerArmDeployedLimit);
	m_currentArmPosition = kBack;
	m_requestedArmPosition = kBack;
	m_propTimer = new Timer();
	
	m_roller = new Talon(c_rollerTalonChannel);
	m_rollerForwardSwitch = new OperatorButton(1, c_rollerForwardSwitch);
	m_rollerReverseSwitch = new OperatorButton(1, c_rollerReverseSwitch);
}

RollerSubsystem::~RollerSubsystem()
{
}

void RollerSubsystem::DoRobotInit(void)
{
	
}

void RollerSubsystem::DoAutonomousInit(void)
{
	m_roller->Set(0);
}

void RollerSubsystem::DoTeleopInit(void)
{
	SetPosition(kBack);
	m_roller->Set(0);
}

void RollerSubsystem::DoDisabledInit(void)
{
	m_armMotor->Set(0);
	m_roller->Set(0);
}

void RollerSubsystem::DoAutonomousPeriodic(void)
{
	
} 

void RollerSubsystem::DoTeleopPeriodic(void)
{
	//ARM
	if (m_pickupOverride->GetBool() == true)
	{
		ManualArm();	
	}
	else
	{
		if (m_prongsUpButton->Get() == OperatorButton::kJustPressed || m_armBackSwitch->Get() == OperatorButton::kJustPressed)
		{
			SetPosition(kBack);
		}
		else if (m_prongsDownButton->Get() == OperatorButton::kJustPressed || m_armDeployedSwitch->Get() == OperatorButton::kJustPressed)
		{
			SetPosition(kDeployed);
		}
		ArmMotorControl();
	}
	
	//ROLLER
	if (m_rollerForwardSwitch->GetBool() == true)
	{
		m_roller->Set(c_rollerSpeed);
	}
	else if(m_rollerReverseSwitch->GetBool() == true)
	{
		m_roller->Set(-1*c_rollerSpeed);
	}
	else
	{
		m_roller->Set(0);
	}
	
	
	
	if (m_backLimitSwitch->Get() == false)
	{
		printf("arm back limit switch is false \n");
	}
	if (m_deployedLimitSwitch->Get() == false)
	{
		printf("arm forward limit switch is false \n");
	}
	
	SmartDashboard::PutBoolean("Arm back: ", !m_backLimitSwitch->Get());
	SmartDashboard::PutBoolean("Arm deployed: ", !m_deployedLimitSwitch->Get());
}

void RollerSubsystem::DoDisabledPeriodic(void)
{
	
}

void RollerSubsystem::SetPosition(ArmPosition armPosition)
{
	m_requestedArmPosition = armPosition;
}

void RollerSubsystem::ArmMotorControl()
{
	if(m_requestedArmPosition == kDeployed)
	{
		if(m_deployedLimitSwitch->Get() == true)
		{
			m_armMotor->Set(c_armSpeed);
		}	
		else if (m_deployedLimitSwitch->Get() == false)
		{
			m_armMotor->Set(0);
		}
	}
	else if(m_requestedArmPosition == kBack)
	{
		if(m_backLimitSwitch->Get() == true)
		{
			m_armMotor->Set(-1*c_armSpeed);
		}
		else if(m_backLimitSwitch->Get() == false)
		{
			m_armMotor->Set(0);
		}
	}
}

void RollerSubsystem::ManualArm(void)
{
	if (m_armDeployedSwitch->GetBool() == true)
	{
		m_armMotor->Set(c_armSpeed);
	}
	else if (m_armBackSwitch->GetBool() == true)
	{
		m_armMotor->Set(-1*c_armSpeed);
	}
	else m_armMotor->Set(0);
}

void RollerSubsystem::DropProp()
{
	m_propTimer->Start();
	
	if (m_propTimer->Get() < c_dropPropTime)
	{
		m_armMotor->Talon::Set(c_armSpeed);
	}
}

bool RollerSubsystem::GetIsArmBack()
{
	if (m_backLimitSwitch->Get() == false)
	{
		return true;
	}
	else
	{
		return false;
	}
}

bool RollerSubsystem::GetIsArmDeployed()
{
	if (m_deployedLimitSwitch->Get() == false)
	{
		return true;
	}
	else
	{
		return false;
	}
}
